#include <behaviortree_cpp/behavior_tree.h>
#include <behaviortree_cpp/bt_factory.h>
#include <rclcpp/rclcpp.hpp>
#include <memory>
#include <rclcpp_action/rclcpp_action.hpp>
#include "../../../install/abb_interface/include/abb_interface/abb_interface/action/abb.hpp"
#include <thread>
#include <chrono>
#include <vector>
#include <iostream>
#include <string>
std::shared_ptr<class MOMAClient> moma_client = nullptr;
std::shared_ptr<class WasherClient> washer_client = nullptr;

class WasherClient : public rclcpp::Node {
public:
    using Abb = abb_interface::action::Abb;
    using GoalHandleRotate = rclcpp_action::ClientGoalHandle<Abb>;
    WasherClient() : Node("washer_client") {
        client_ = rclcpp_action::create_client<Abb>(this, "lyl_device_washer_ros2");
    }
    void send_goal_task(std::string _task_id, std::string _job_id, const std::vector<std::string> & _params) {
        if(!client_->wait_for_action_server(std::chrono::seconds(10))) {
            RCLCPP_INFO(this->get_logger(), "Washer not connected!");
            return;
        }
    auto goal_request = abb_interface::action::Abb::Goal();
    goal_request.task_request.task_id = _task_id;
    goal_request.task_request.job_id = _job_id;
    goal_request.task_request.params = _params;

    RCLCPP_INFO(this->get_logger(), "sending message to Washer");

    auto send_goal_options = rclcpp_action::Client<Abb>::SendGoalOptions();
    send_goal_options.result_callback = std::bind(&WasherClient::result_callback, this, std::placeholders::_1);

    client_->async_send_goal(goal_request, send_goal_options);
    }

    void result_callback(const GoalHandleRotate::WrappedResult & result) {
        switch(result.code) {
            case rclcpp_action::ResultCode::SUCCEEDED:
                RCLCPP_INFO(this->get_logger(), "Washer task completed!");
                break;
        default:
            RCLCPP_INFO(this->get_logger(), "Washer task error!");
            break;
        }
    }

    rclcpp_action::Client<Abb>::SharedPtr client_;
};

class MOMAClient : public rclcpp::Node {
public:
    using Abb = abb_interface::action::Abb;
    using GoalHandleRotate = rclcpp_action::ClientGoalHandle<Abb>;
    MOMAClient() : Node("moma_client") {
        client_ = rclcpp_action::create_client<Abb>(this, "wsl_device_MOMA_ros2");
    }

    void send_goal_task(std::string _task_id, std::string _job_id, const std::vector<std::string> & _params) {
        if(!client_->wait_for_action_server(std::chrono::seconds(10))) {
            RCLCPP_INFO(this->get_logger(), "MOMA not connected!");
            return;
        }

        auto goal_request = abb_interface::action::Abb::Goal();
        goal_request.task_request.task_id = _task_id;
        goal_request.task_request.job_id = _job_id;
        goal_request.task_request.params = _params;
        
        RCLCPP_INFO(this->get_logger(), "sending message to MOMA");

        auto send_goal_options = rclcpp_action::Client<Abb>::SendGoalOptions();
        send_goal_options.result_callback = std::bind(&MOMAClient::result_callback, this, std::placeholders::_1);

        client_->async_send_goal(goal_request, send_goal_options);
    }

    void result_callback(const GoalHandleRotate::WrappedResult & result) {
        switch(result.code) {
            case rclcpp_action::ResultCode::SUCCEEDED:
            RCLCPP_INFO(this->get_logger(), "MOMA task completed!");
        break;
        default:
            RCLCPP_INFO(this->get_logger(), "MOMA task error!");
            break;
        }
    }
    rclcpp_action::Client<Abb>::SharedPtr client_;
};
//树节点对象
class MoveToWasher : public BT::SyncActionNode {
public:
    MoveToWasher(const std::string& name) : BT::SyncActionNode(name, {}) {}
    BT::NodeStatus tick() override {
        std::vector<std::string> params = {"washer"};
        std::cout << "MOMA前往洗瓶机\n"<< std::endl;
        moma_client->send_goal_task("1", "3001", params);
        return BT::NodeStatus::SUCCESS;
    }
};

// 异步移动节点：发送指令后等待结果
class MoveToPutMaterial : public BT::AsyncActionNode {
public:
    MoveToPutMaterial(const std::string& name, const BT::NodeConfig& config)
        : BT::AsyncActionNode(name, config) {}

    // 静态函数：定义节点输入输出端口（如需传递参数）
    static BT::PortsList providedPorts() {
        return {};
    }

    // 异步执行：发送任务后等待结果
    BT::NodeStatus tick() override {
        std::vector<std::string> params = {"put_material_point"};
        std::cout << "MOMA前往上料点（等待到达确认）\n" << std::endl;

        // 记录结果的原子变量
        std::atomic<bool> task_success(false);
        std::atomic<bool> task_complete(false);

        // 发送任务并设置结果回调
        auto send_goal_options = rclcpp_action::Client<abb_interface::action::Abb>::SendGoalOptions();
        send_goal_options.result_callback = 
            [&](const rclcpp_action::ClientGoalHandle<abb_interface::action::Abb>::WrappedResult& result) {
                switch(result.code) {
                    case rclcpp_action::ResultCode::SUCCEEDED:
                        task_success = result.result->task_response.task_state;
                        task_complete = true;
                        break;
                    default:
                        task_success = false;
                        task_complete = true;
                        break;
                }
            };

        // 发送目标任务
        auto goal_request = abb_interface::action::Abb::Goal();
        goal_request.task_request.task_id = "2";
        goal_request.task_request.job_id = "3001";
        goal_request.task_request.params = params;
        moma_client->client_->async_send_goal(goal_request, send_goal_options);

        // 等待结果（超时60秒，与robot_node保持一致）
        auto start_time = std::chrono::steady_clock::now();
        while(!task_complete) {
            if(std::chrono::steady_clock::now() - start_time > std::chrono::seconds(60)) {
                std::cout << "MOMA前往上料点超时\n" << std::endl;
                return BT::NodeStatus::FAILURE;
            }
            std::this_thread::sleep_for(std::chrono::milliseconds(100));
        }

        // 根据结果返回行为树状态
        if(task_success) {
            std::cout << "MOMA已到达上料点\n" << std::endl;
            return BT::NodeStatus::SUCCESS;
        } else {
            std::cout << "MOMA未到达上料点\n" << std::endl;
            return BT::NodeStatus::FAILURE;
        }
    }

    void halt() override {
        // 如需支持任务取消，可在此处实现
        std::cout << "MoveToPutMaterial任务被中断\n" << std::endl;
        BT::AsyncActionNode::halt();
    }
};


class PlaceBottle1 : public BT::SyncActionNode {
public:
    PlaceBottle1(const std::string& name) : BT::SyncActionNode(name, {}) {}
    BT::NodeStatus tick() override {
        std::vector<std::string> params = {"1", "1"};
        std::cout << "MOMA摆放瓶子1\n" << std::endl;
        moma_client->send_goal_task("4", "3002", params);
        return BT::NodeStatus::SUCCESS;
    }
};

class PlaceBottle2 : public BT::SyncActionNode {
public:
    PlaceBottle2(const std::string& name) : BT::SyncActionNode(name, {}) {}
    BT::NodeStatus tick() override {
        std::vector<std::string> params = {"1", "2"};
        std::cout << "MOMA摆放瓶子2\n" << std::endl;
        moma_client->send_goal_task("5", "3002", params);
        return BT::NodeStatus::SUCCESS;
    }
};

class PlaceBottle3 : public BT::SyncActionNode {
public:
    PlaceBottle3(const std::string& name) : BT::SyncActionNode(name, {}) {}
    BT::NodeStatus tick() override {
        std::vector<std::string> params = {"1", "3"};
        std::cout << "MOMA摆放瓶子3\n" << std::endl;
        moma_client->send_goal_task("6", "3002", params);
        return BT::NodeStatus::SUCCESS;
    }
};

class PlaceBottle4 : public BT::SyncActionNode {
    public:
    PlaceBottle4(const std::string& name) : BT::SyncActionNode(name, {}) {}
    BT::NodeStatus tick() override {
        std::vector<std::string> params = {"1", "4"};
        std::cout << "MOMA摆放瓶子4\n" << std::endl;
        moma_client->send_goal_task("7", "3002", params);
        return BT::NodeStatus::SUCCESS;
    }
};

class ReturnToHome : public BT::SyncActionNode {
public:
    ReturnToHome(const std::string& name) : BT::SyncActionNode(name, {}) {}
    BT::NodeStatus tick() override {
        std::vector<std::string> params = {"home"};
        std::cout << "MOMA返回HOME点\n" << std::endl;
        moma_client->send_goal_task("10", "3001", params);
        return BT::NodeStatus::SUCCESS;
    }
};



//充电指令 true充电 false停止充电
class AGVChargeOn : public BT::SyncActionNode {
public:
    AGVChargeOn(const std::string& name) : BT::SyncActionNode(name, {}) {}
    BT::NodeStatus tick() override {
        std::vector<std::string> params = {"true"};
        std::cout << "MOMA开始充电\n" << std::endl;
        moma_client->send_goal_task("1", "3004", params);
        return BT::NodeStatus::SUCCESS;
    }
};

class AGVChargeOff : public BT::SyncActionNode {
public:
    AGVChargeOff(const std::string& name) : BT::SyncActionNode(name, {}) {}
    BT::NodeStatus tick() override {
        std::vector<std::string> params = {"false"};
        std::cout << "MOMA停止充电\n" << std::endl;
        moma_client->send_goal_task("1", "3004", params);
        return BT::NodeStatus::SUCCESS;
    }
};

class CheckDeviceStatus : public BT::SyncActionNode {
public:
    CheckDeviceStatus(const std::string& name) : BT::SyncActionNode(name, {}) {}
    BT::NodeStatus tick() override {
        std::vector<std::string> params = {"device"};
        std::cout << "MOMA返回设备状态\n" << std::endl;
        moma_client->send_goal_task("1", "3003", params);
        return BT::NodeStatus::SUCCESS;
    }
};

class CheckWorkStatus : public BT::SyncActionNode {
public:
    CheckWorkStatus(const std::string& name) : BT::SyncActionNode(name, {}) {}
    BT::NodeStatus tick() override {
        std::vector<std::string> params = {"work"};
        std::cout << "MOMA返回工作状态\n" << std::endl;
        moma_client->send_goal_task("2", "3003", params);
        return BT::NodeStatus::SUCCESS;
    }
};


class GetAGVStation : public BT::SyncActionNode {
public:
    GetAGVStation(const std::string& name) : BT::SyncActionNode(name, {}) {}
    BT::NodeStatus tick() override {
        std::vector<std::string> params = {"station"};
        std::cout << "MOMA返回当前站点\n" << std::endl;
        moma_client->send_goal_task("3", "3003", params);
        return BT::NodeStatus::SUCCESS;
    }
};

class GetAGVPower : public BT::SyncActionNode {
public:
    GetAGVPower(const std::string& name) : BT::SyncActionNode(name, {}) {}
    BT::NodeStatus tick() override {
        std::vector<std::string> params = {"power"};
        std::cout << "MOMA返回电量\n" << std::endl;
        moma_client->send_goal_task("4", "3003", params);
        return BT::NodeStatus::SUCCESS;
    }
};

class CheckChargeStatus : public BT::SyncActionNode {
public:
    CheckChargeStatus(const std::string& name) : BT::SyncActionNode(name, {}) {}
    BT::NodeStatus tick() override {
        std::vector<std::string> params = {"charge"};
        std::cout << "MOMA返回充电状态\n" << std::endl;
        moma_client->send_goal_task("5", "3003", params);
        return BT::NodeStatus::SUCCESS;
    }
};

class CheckProgStatus : public BT::SyncActionNode {
public:
    CheckProgStatus(const std::string& name) : BT::SyncActionNode(name, {}) {}
    BT::NodeStatus tick() override {
        std::vector<std::string> params = {"program"};
        std::cout << "MOMA返回程序运行情况\n" << std::endl;
        moma_client->send_goal_task("6", "3003", params);
        return BT::NodeStatus::SUCCESS;
    }
};

//scheduler调度器对象
class SchedulerTest01 : public rclcpp::Node {
public:
    SchedulerTest01(std::string name) : Node(name) {
    // 初始化行为树
        initBehaviorTree();
    }

private:
    void initBehaviorTree() {
    // 创建行为树工厂对象
        BT::BehaviorTreeFactory factory;
                
        // 注册所有任务节点
        // 有几个任务就需要有几个任务节点
        factory.registerNodeType<MoveToWasher>("MoveToWasher");
        factory.registerNodeType<MoveToPutMaterial>("MoveToPutMaterial");
        factory.registerNodeType<PlaceBottle1>("PlaceBottle1");
        factory.registerNodeType<PlaceBottle2>("PlaceBottle2");
        factory.registerNodeType<PlaceBottle3>("PlaceBottle3");
        factory.registerNodeType<PlaceBottle4>("PlaceBottle4");
        factory.registerNodeType<ReturnToHome>("ReturnToHome");

        factory.registerNodeType<GetAGVPower>("GetAGVPower");
        factory.registerNodeType<GetAGVStation>("GetAGVStation");
        factory.registerNodeType<CheckDeviceStatus>("CheckDeviceStatus");
        factory.registerNodeType<CheckWorkStatus>("CheckWorkStatus");
        factory.registerNodeType<CheckChargeStatus>("CheckChargeStatus");
        factory.registerNodeType<CheckProgStatus>("CheckProgStatus");

        std::string xml_text = R"(
        <root BTCPP_format="4">
            <BehaviorTree ID="MainTree">
                <Sequence>
                    <ReturnToHome />
                </Sequence>
            </BehaviorTree>
        </root>
        )";
                
        // 创建并执行行为树
        tree_ = factory.createTreeFromText(xml_text);
        tree_.tickWhileRunning();
    }
    BT::Tree tree_;
};

int main(int argc, char** argv) {
    rclcpp::init(argc, argv);

    moma_client = std::make_shared<MOMAClient>();

    auto node1 = std::make_shared<SchedulerTest01>("sys_scheduler01");

    rclcpp::executors::MultiThreadedExecutor executor;
    executor.add_node(node1);
    executor.add_node(moma_client);

    // 启动执行器（会阻塞直到收到终止信号）
    executor.spin();

    moma_client.reset();

    rclcpp::shutdown();
    return 0;
}